using BepuPhysics.Collidables;
using BepuUtilities;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.SweepTasks
{
    public struct CapsulePairDistanceTester : IPairDistanceTester<CapsuleWide, CapsuleWide>
    {
        public void Test(in CapsuleWide a, in CapsuleWide b, in Vector3Wide offsetB, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector<int> inactiveLanes,
            out Vector<int> intersected, out Vector<float> distance, out Vector3Wide closestA, out Vector3Wide normal)
        {
            // 计算两条直线段之间最近的点。一开始就不要夹紧。
            // 我们希望最小化距离=||(a+da*ta)-(b+db*tb)||。
            // 计算关于ta的导数并进行一些代数运算(考虑||da||=||db||==1)来求解ta的结果：
            // Ta=(da*(b-a)+(db*(a-b))*(da*db))/(1-((da*db)*(da*db)
            QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
            QuaternionWide.TransformUnitY(orientationB, out var db);
            Vector3Wide.Dot(da, offsetB, out var daOffsetB);
            Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
            Vector3Wide.Dot(da, db, out var dadb);
            // 请注意,当轴线平行时,电势除以零。任意钳位;接近零值会产生极值,而这些极值被钳位到合理的结果。
            var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector<float>(1e-15f), Vector<float>.One - dadb * dadb);
            // tb=ta*(da*db)-db*(b-a)
            var tb = ta * dadb - dbOffsetB;

            // 我们不能简单地将ta和tb值夹在胶囊线段上。取而代之的是,将每条线段投影到另一条线段上,夹紧目标的间隔。
            // 该新的钳制投影间隔是该直线段上的有效解空间。我们可以用这个区间钳制t值,得到正确的有界解。
            // 预计间隔为：
            // B到A：+-BHalfLength*(da*db)+da*offsetB
            // A到B：+-AHalfLength*(da*db)-db*offsetB
            var absdadb = Vector.Abs(dadb);
            var bOntoAOffset = b.HalfLength * absdadb;
            var aOntoBOffset = a.HalfLength * absdadb;
            var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
            var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
            var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
            var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
            ta = Vector.Min(Vector.Max(ta, aMin), aMax);
            tb = Vector.Min(Vector.Max(tb, bMin), bMax);

            Vector3Wide.Scale(da, ta, out closestA);
            Vector3Wide.Scale(db, tb, out var closestB);
            Vector3Wide.Add(closestB, offsetB, out closestB);

            Vector3Wide.Subtract(closestA, closestB, out normal);
            Vector3Wide.Length(normal, out distance);
            var inverseDistance = Vector<float>.One / distance;
            Vector3Wide.Scale(normal, inverseDistance, out normal);
            Vector3Wide.Scale(normal, a.Radius, out var aOffset);
            Vector3Wide.Subtract(closestA, aOffset, out closestA);
            distance = distance - a.Radius - b.Radius;
            intersected = Vector.LessThanOrEqual(distance, Vector<float>.Zero);
        }
    }

}
